#include <sstream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include "ContactManager.h"

ContactManager::ContactManager(bool showGui)
	: showGui(showGui),
	imgBackground(),
	imgWorkingA(),
	imgWorkingB(),
	idCounter(0),
	contacts()
{
	server = new TUIO::TuioServer();

	rectifyXMin = -11.0f / 320.0f;
	rectifyXMax = 10.0f / 320.0f;
	rectifyYMin = -8.0f / 240.0f;
	rectifyYMax = 20.0f / 240.0f;

	paramBlurSize = 10.6f; // 9.0f;
	paramThreshold = 1.4f; // 2.0f;
}

ContactManager::~ContactManager()
{
	delete server;
}

void ContactManager::init(const cv::Mat& imgCurrent)
{
	imgBackground = imgCurrent.clone();
}

void ContactManager::update(const cv::Mat& imgCurrent)
{
	// update params
	int key = cv::waitKey(1);
	if(key == '+')
	{
		paramThreshold += 0.2f;
		std::cout << "threshold: " << paramThreshold << std::endl;
	}
	if(key == '-')
	{
		paramThreshold -= 0.2f;
		std::cout << "threshold: " << paramThreshold << std::endl;
	}
	if(key == '1')
	{
		paramBlurSize -= 0.1f;
		std::cout << "blur: " << paramBlurSize << std::endl;
	}
	if(key == '2')
	{
		paramBlurSize += 0.1f;
		std::cout << "blur: " << paramBlurSize << std::endl;
	}
	if(key == 'r')
	{
		imgBackground = imgCurrent;
		std::cout << "background reset" << std::endl;
	}


	// 
	TUIO::TuioTime time = TUIO::TuioTime::getSessionTime();
	server->initFrame(time);

	for(auto it = contacts.begin(); it != contacts.end(); it++)
	{
		ContactPtr contact = *it;
		contact->timeout--;
		contact->assigned = false;
	}

	// background sub
	cv::absdiff(imgCurrent, imgBackground, imgWorkingA);
	// highpass
	cv::Size blurSize(paramBlurSize,paramBlurSize);
	cv::blur(imgWorkingA, imgWorkingB, blurSize);
	cv::absdiff(imgWorkingA, imgWorkingB, imgWorkingA);
	cv::blur(imgWorkingA, imgWorkingB, blurSize);
	cv::threshold(imgWorkingB, imgWorkingA, paramThreshold, 255, CV_THRESH_BINARY);
	// contour
	cv::vector<cv::vector<cv::Point>> contours;
	cv::vector<cv::Vec4i> hierarchy;
	cv::findContours(imgWorkingA, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );

	cv::Mat imgCurrentCopy;
	if(showGui)
	{
		imgCurrentCopy = imgCurrent.clone();
		cv::cvtColor(imgCurrentCopy, imgCurrentCopy, CV_GRAY2RGB);
	}
	// iterate through all the top-level contours -> "hierarchy" may not be empty!)
	if( hierarchy.size() > 0 )
	{
		const int width = imgBackground.size().width;
		const int height = imgBackground.size().height;
		for( int idx = 0; idx >= 0; idx = hierarchy[idx][0] )
		{
			if (cv::contourArea(cv::Mat(contours.at(idx))) > 30) // check contour size
			{
				cv::RotatedRect point = cv::fitEllipse(cv::Mat(contours.at(idx)));
				float relX = point.center.x / (float)width;
				float relY = point.center.y / (float)height;
				point.center.x += (rectifyXMin * (1.0f - relX) + rectifyXMax * relX) * (float)width;
				point.center.y += (rectifyYMin * (1.0f - relY) + rectifyYMax * relY) * (float)height;

				if(showGui)
				{
					// fit & draw ellipse to contour at index "idx"
					cv::ellipse(imgCurrentCopy, point, cv::Scalar(0,0,255), 1, 8);
					// draw contour at index "idx"
					drawContours(imgCurrentCopy, contours, idx, cv::Scalar(255,0,0), 1, 8, hierarchy);
				}

				updateContact(point);
			}
		}
	}

	for(auto it = contacts.begin(); it != contacts.end();)
	{
		ContactPtr contact = *it;
		if(!contact->timeout)
		{
			server->removeTuioCursor(contact->tuioHandle);
			it = contacts.erase(it);
		}
		else
		{
			if(showGui) {
				std::stringstream idStream;
				idStream << "id: " << contact->id;
				cv::putText(imgCurrentCopy, idStream.str() , cv::Point((int)contact->posX, (int)contact->posY), CV_FONT_HERSHEY_PLAIN, 1.0, CV_FONT_BLACK);
			}
			it++;
		}
	}

	server->stopUntouchedMovingCursors();
	server->commitFrame();
	if(showGui)
		cv::imshow("result", imgCurrentCopy);
}

ContactPtr ContactManager::updateContact(const cv::RotatedRect& ellipse)
{
	const int width = imgBackground.size().width;
	const int height = imgBackground.size().height;
	const float tolerance = 20.f;

	ContactPtr bestContact;
	float bestDistance = std::numeric_limits<float>::infinity();

	cv::Point2f contactPoint = ellipse.center;
	float size = (ellipse.size.width + ellipse.size.height) / 2.f;
	for(auto it = contacts.begin(); it != contacts.end(); it++)
	{
		ContactPtr c = *it;
		float dX = c->posX - contactPoint.x;
		float dY = c->posY - contactPoint.y;
		// position estimation
		dX += c->velX;
		dY += c->velY;

		float distance = std::sqrt(dX * dX + dY * dY);
		if(!c->assigned && distance < bestDistance)
		{
			bestContact = c;
			bestDistance = distance;
		}
	}

	if(bestContact && bestDistance <= tolerance)
	{
		// update
		bestContact->assigned = true;
		bestContact->timeout++;
		bestContact->velX = contactPoint.x - bestContact->posX;
		bestContact->velY = contactPoint.y - bestContact->posY;

		bestContact->posX = contactPoint.x;
		bestContact->posY = contactPoint.y;

		server->updateTuioCursor(bestContact->tuioHandle, bestContact->posX / (float) width, bestContact->posY / (float) height);
	}
	else
	{
		// insert new
		bestContact = ContactPtr(new Contact(idCounter++, contactPoint.x, contactPoint.y, size, 0.f));
		bestContact->tuioHandle = server->addTuioCursor(contactPoint.x / (float) width, contactPoint.y / (float) height);
		contacts.push_back(bestContact);
	}
	return bestContact;
}